﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;

using GoblinXNA;
using GoblinXNA.SceneGraph;
using GoblinXNA.Graphics;
using GoblinXNA.Graphics.ParticleEffects;
using GoblinXNA.Device.Capture;
using GoblinXNA.Device.Vision.Marker;
using GoblinXNA.Device.Generic;
using GoblinXNA.Device;
using Model = GoblinXNA.Graphics.Model;
using GoblinXNA.Graphics.Geometry;
using GoblinXNA.Physics;
using GoblinXNA.Physics.Newton1;
using GoblinXNA.Helpers;
using GoblinXNA.UI;

using NewtonDynamics;

namespace AugmentedRealityTeam
{
    public class RaceCar
    {
        public static float PER_MASS_SPEED = 200.0f;
        //public static float MAX_STEER_ANGLE = 30.0f * MathHelper.Pi / 60.0f;

        public float forceX = 0;
        public float forceY = 0;
        public float speedX = 0;
        public float speedY = 0;
        //public float angle = 0; //mesured from 0 to 359 degree
        public static float MAX_ANGLE = 60.0f; //maximum angle
        public static float MIN_ANGLE = -60.0f; //minimum angle

        public Scene scene;
        public MarkerNode gMarkerNode;
        public WallCreator wallCreator;
        //ObjectCreator //for example cubes, spheres
        public GeometryNode realGroNode;
        public GeometryNode gNode;

        public GeometryNode carNode;
        public TransformNode carTransNode;
        public Matrix carPhysicsTranslationMat;

        Vector3 translationVec;
        Quaternion rotaionVec;
        Vector3 scalingVec;
        Material carMaterial;

        float trunningDegree = 0;
        float scaleObjRatio = 5.0f;

        public void adjustRaceCarVisible(bool visible)
        {
            if (visible)
                carMaterial.Diffuse = new Vector4(0, 0, 0, 0);
            else
                carMaterial.Diffuse = new Vector4(0.6f, 0.5f, 0.5f, 0.8f);
        }

        public RaceCar(WallCreator wCreator, Scene sce, MarkerNode groundMarkerNode,
            GeometryNode realGroundNode, GeometryNode groundNode, float carTranslationX, float carTranslationY)
        {
            scene = sce;
            gMarkerNode = groundMarkerNode;
            gNode = groundNode;

            wallCreator = wCreator;
            realGroNode = realGroundNode;
            //ObjectCreator //for example cubes, spheres
            createCar(carTranslationX, carTranslationY);

            translationVec = new Vector3();
            rotaionVec = new Quaternion();
            scalingVec = new Vector3();
        }

        public void createCar(float carTranslationX, float carTranslationY)
        {
            carNode = new GeometryNode("car");
            //carNode.Model = new Box(8);
            carNode.Model = new Box(8,3, 1);

            carNode.Physics.MaterialName = "car";
            // carNode.Physics.Interactable = true;
            carNode.Physics.Interactable = true;
            carNode.Physics.Collidable = true;
            //carNode.Physics.Collidable = false;
            carNode.Physics.Shape = GoblinXNA.Physics.ShapeType.Box;
            carNode.Physics.Mass = 60f;
            carNode.Physics.AngularDamping = new Vector3(.1f, .1f, .1f);
            carNode.Physics.LinearDamping = 2.5f;
            carNode.AddToPhysicsEngine = true;

            carNode.Model.CastShadows = true;
            carNode.Model.ReceiveShadows = true;

            carMaterial = new Material();
            carMaterial.Diffuse = new Vector4(0.6f, 0.5f, 0.5f, 0.8f);
            carMaterial.Specular = Color.White.ToVector4();
            carMaterial.SpecularPower = 10;
            carNode.Material = carMaterial;
            carTransNode = new TransformNode();

            //car initial position
            carTranslationX = 30f;
            carTranslationY = -30f;
            carTransNode.Translation = new Vector3(carTranslationX * scaleObjRatio, carTranslationY * scaleObjRatio, 2.5f);


            gNode.AddChild(carTransNode);
            carTransNode.AddChild(carNode);

            //NOTE there must be no scaling and rotation to the carNode
            carPhysicsTranslationMat = carNode.Physics.PhysicsWorldTransform;
        }

        //reinit the racecar, update the new wall, ground and object information
        public void reinit(WallCreator wCreator)
        {
            wallCreator = wCreator;
            //ObjectCreator //for example cubes, spheres            
        }

        public void updateForce(float LinearSpeed, float rotAngle)
        {
            Matrix translationMatrix = new Matrix();
            Matrix rotaionMatrix = new Matrix();

            Matrix.CreateTranslation(carNode.Physics.PhysicsWorldTransform.Translation.X,
                carNode.Physics.PhysicsWorldTransform.Translation.Y,
                carNode.Physics.PhysicsWorldTransform.Translation.Z, out translationMatrix);


            rotaionMatrix = realGroNode.Physics.PhysicsWorldTransform * Matrix.Invert(translationMatrix);

            //Console.WriteLine("M21  " + rotaionMatrix.M21);
            //Console.WriteLine("M11  " + rotaionMatrix.M11);

            //forceX = FORCE_PER_MASS_SPEED * LinearSpeed * rotaionMatrix.M21;
            //forceY = FORCE_PER_MASS_SPEED * LinearSpeed * rotaionMatrix.M11;

            //forceX = FORCE_PER_MASS_SPEED * LinearSpeed;
            //forceY = FORCE_PER_MASS_SPEED * LinearSpeed;


            speedX = PER_MASS_SPEED * LinearSpeed;


            //apply force to the virtual ground
            Vector3 speedVec = Vector3.Transform(new Vector3(-speedX, 0, 0), Matrix.CreateRotationY((float)rotAngle));
            ((NewtonPhysics)scene.PhysicsEngine).AddForce(wallCreator.realgNode.Physics, speedVec);

            //apply force to all the walls
            for (int i = 0; wallCreator.wallNode[i] != null && i < wallCreator.wallindex; i++)
            {
                ((NewtonPhysics)scene.PhysicsEngine).AddForce(wallCreator.wallNode[i].Physics, speedVec);
            }
        }

        public void turn()
        {
            Matrix translationMatrix = realGroNode.Physics.PhysicsWorldTransform;
            Matrix rotaionMatrix = new Matrix();
            Matrix.CreateRotationZ(trunningDegree, out rotaionMatrix);

            //realGroNode.Physics.PhysicsWorldTransform = realGroNode.Physics.PhysicsWorldTransform
            //    * Matrix.Invert(translationMatrix)
            //    * rotaionMatrix
            //    * translationMatrix;

            realGroNode.Physics.PhysicsWorldTransform = translationMatrix
                * rotaionMatrix
                * Matrix.Invert(translationMatrix)
                * realGroNode.Physics.PhysicsWorldTransform;

            trunningDegree = trunningDegree + .1f;
            Console.WriteLine("trunningDegree     " + trunningDegree);
        }

        public float GetSpeed()
        {
            return Vector3.Dot(wallCreator.realgNode.Physics.PhysicsWorldTransform.Forward,
                ((NewtonPhysics)scene.PhysicsEngine).GetVelocity(wallCreator.realgNode.Physics));
        }









    }
}
